#include <opencv2/opencv.hpp>
 
/*
"**" after "//" means the values are relative to Anti-Kalman regression speed and radical range.
*/
 
using namespace std;
using namespace cv;
 
namespace Kalman_example
{
class KalmanFilter
{
public:
	KalmanFilter() {}
        /*
		KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F )
		"dynamParams = 4": 4*1 vector of state (x, y, delta x, delta y)
		"measureParams = 2": 2*1 vector of measurement (x, y)
		*/
    KalmanFilterInit(float x, float y)
    {   
        KF(4, 2);
        measurement_ = Mat::zeros(2, 1, CV_32F);// (x, y)
        KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 1, 0,//**Latter 1: Larger, faster regression
                                                         0, 1, 0, 1,//**Latter 1: Larger, faster regression
                                                         0, 0, 1, 0,
                                                         0, 0, 0, 1);
         setIdentity(KF.measurementMatrix, Scalar::all(1));
         setIdentity(KF.processNoiseCov, Scalar::all(1e-10));//**10: Larger, slower regression
        setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));//1: Larger, quicker regression
        setIdentity(KF.errorCovPost, Scalar::all(1));
        KF.statePost = (Mat_<float>(4, 1) << x, y, 0, 0);//Ensure beginner is default value
    }

 
	Point2f run(float x, float y)
	{
		Mat prediction = KF.predict();
		Point2f predict_pt = Point2f(prediction.at<float>(0),prediction.at<float>(1));
 
		measurement_.at<float>(0, 0) = x;
		measurement_.at<float>(1, 0) = y;
 
		KF.correct(measurement_);
 
		return predict_pt;
	}
private:
	Mat measurement_;
	cv::KalmanFilter KF;//Differ from Kalman_example::KalmanFilter
};
 
}
